

namespace Diversity.Robotics.Navigation
{
    // TODO, this is too complex, find a simpler way
    // that factors out the beam and cone models somehow.
    // I wanted to be able to mix different sensors in one array.

    /// <summary>
    /// A Distance Reading with details of angle, origin and interpretation
    /// </summary>
    public class DistanceReading
    {
        private double _x;
        private double _y;
        private double _theta;
        private double _distance;
        private BeamModel _beamModel;
        private IConeModel _coneModel;

        public double X
        {
            get { return _x; }
            set { _x = value; }
        }

        public double Y
        {
            get { return _y; }
            set { _y = value; }
        }

        public double Theta
        {
            get { return _theta; }
            set { _theta = value; }
        }

        public double Distance
        {
            get { return _distance; }
            set { _distance = value; }
        }

        public BeamModel Beam
        {
            get { return _beamModel; }
            set { _beamModel = value; }
        }

        public IConeModel Cone
        {
            get { return _coneModel; }
            set { _coneModel = value; }
        }

        public DistanceReading(double x, double y, double theta, double distance, BeamModel beamModel, IConeModel coneModel)
        {
            _x = x;
            _y = y;
            _theta = theta;
            _distance = distance;
            _beamModel = beamModel;
            _coneModel = coneModel;
        }

        public void UpdateMap(ref double[,] occupancy, double scale)
        {
            _coneModel.UpdateMap(ref occupancy, this, scale);
        }

        public override string ToString()
        {
            return "DistanceReading ( x: " + _x + " ,y: " + _y + " ,theta: " + _theta + " ,distance: " + _distance + ")";
        }
    }
}
